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ABSTRACT 


A terminal guidance and navigation scheme developed in 
earlier work is modified and evaluated for a solar electric 
propulsion rendezvous mission to comet Encke. The scheme 
is intended for autonomous, on board use. The guidance al- 
gorithm is based in optimal control theory and minimizes 
the time integrated square of thrust acceleration. The 
navigation algorithm employs a modified Kalman filter set in 
measurement variables . Random sequences were generated to 
simulate measurement errors and the evaluation was conducted 
with detailed numerical computations which include actual 
motions of spacecraft and comet. The evaluations showed 
that the. scheme attains rendezvous and maintains station 
after rendezvous within less than 10 km for estimated best 
measurements and within less than 100 km for estimated 
"worst" measurements. The measurements required are angles, 
range and range rate. Angles and range appear to be abso- 
lutely necessary. Range rate is not as strong a measurement 
type and further modifications of the filter will quite 
probably allow a scheme that does not require the rate 
measurements . 
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1.0 INTRODUCTION 


In previous work on the comet and asteroid navigation and guidance 
problem, an optimal control theory guidance algorithm was devised.^ An 
evaluation of this algorithm combined v;ith a Kalman filter method for 
navigation showed that onboard guidance and navigation is possible with 
reasonable limits on measurement errors.^ But, this evaluation did not 
fully establish practicality of the onboard approach. Questions of how 
many measurement types are required and the effects of range of measure- 
ment accuracy were not answered. And, the particular coordinate system 
employed (range and direction cosines) gave unacceptable singularities 
near coordinate planes. Also, there was the not unusual question of 
management of state estimate divergence caused by the use of a linear 
filter with a nonlinear system. 

The objective of the work presented here was to carry out further 
algorithm development to provide first answers to the questions above. 
Results were to Include the precision attainable with different instru- 
ment types and accuracies and the onboard computer requirements neces- 
sary to implement the scheme. 

After Initiation of the work, it became evident that the conversion 
of the previously developed GANDER guidance and navigation computer 
program to a non-singular coordinate system would be a larger task than 
originally planned. The divergence question also presented unforeseen 
difficulties. It was therefore agreed to reduce work on less important 
tasks. Specifically, thrust errors were simulated by statistical terms 
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in the filter equations and not by a second method wherein thrust errors 
would be statistically developed and handled as new state variables. 
Similarly, the use of available angular measurements to obtain improved 
estimates of target ephemerls was not investigated. These refinements, 
properly done, can only improve results. Since successful rendezvous 
was obtained without these refinements, the decision to drop them was 

correct . 

Identification of specific Instruments, their accuracies and power 
requirements could not be carried out because the requisite data has 
yet to be developed in the frequency ranges of interest for onboard 
radar in free space. A request to do this data development could not 
be funded, so these questions remain open. 
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2.0 REFORMULATION OF THE GUIDANCE AND NAVIGATION SCHEME 

A firs L step in the work reported here was to eliminate the dif- 
ficulties encountered with the range and direction cosine coordinates 
used previously in the Kalman filter. The difficulty arose because of 
the basic relation among direction cosines that the sum of their 
squares is unity. Near a coordinate plane one of the direction cosines 
approaches zero and small numerical differences such as arise from 
measurement errors can lead, in numerical computation, to imaginary 
values for this cosine. If this happens, the computer becomes most 
upset and reports in displeasure with a long string of error statements . 
There are ways to get around this problem, but in doing so some of the 
information in the data is lost. It is better to employ coordinates 
in which the singularity trouble does not arise. Plain rectangular 
coordinates would solve this problem, but would introduce a nonlinear 
relation between measurement variables and filter variables. It was 
one of our basic objectives to avoid this nonlinearity. 

Coordinates that can be related to measurement variables in a one- 
to-one fashion are simple spherical coordinates, and it is these co- 
ordinates that were decided upon. A singularity in the various func- 
tions arising in the algorithms can arise only on the polar axis of 
the spherical coordinates. This singularity is not only less likely to 
occur, but is more easily managed if it should become necessary. 
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2.1 System Dynamics and the Guidance Algorithm 

We here repeat the equations of motion and guidance algorithm as 

previously developed. ^ 



Figure 1. Rendezvous Geometry. 


Appropriate to onboard guidance, the equations of motion are set 
in a coordinate frame fixed relative to the target as shown in Figure 1. 
In rectangular coordinates centered at the rendezvous point, the equa- 
tions of motion, neglecting the gravitational attraction of the target. 


are 
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^4 “ [S3^“I>i(S/D)31 
*5 " ^2 ^ [S2-D2(S/D)3] 
^6 ^ ^3 ^ [Sg-DgCS/D)^] 


( 1 ) 


where the subscripts indicate components in the corresponding coordinate 
directions. M is the mass of the sun and G is the universal gravitational 
constant. From the optimal control theory guidance law, the control 
forces, F^, F2, F^, which constitute the guidance algorithm, are 



a . |L)].^^ (1 - 

T ^ 0 0 0 

0 

(1 - +lf (1 - f)lx5o 

T 0 0 0 

0 

1 ~ a - f )] x 30 +(^1 - 

T ^ 0 00 


( 2 ) 


o 

where , X2Q, etc., are the initial conditions and x = - T is the 

time to go with T^, = final rendezvous time and T = running time. 

Note that the equations above are in rectangular coordinates. 
These coordinates are employed for the precision integration made to 
construct the comparison trajectory for the evaluation to be made. 
Transformation to the spherical coordinates for the measurement and 
filter variables are made in the navigation algorithm. 
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2.2 The Navigation Algorithm in. Spherical Coordinates. 

As in previous work a procedure devised by Mehra^ was chosen to 
handle nonlinearities of the state-measurement transformation. In this 
procedure, the filtering is carried out in measurement variable coordi- 
nates where the observation transformation is linear. 

The state of the system, x = (x^X 2 X^x^XgX^) , is transformed from 
rectangular coordinates used for the guidance problem to spherical co- 
ordinates, y = (RXSRAS)^ as defined in Figure 2. Measurements of, for 
example, range, range rate, and angles are a subset of the spherical 
coordinates. For this reason the y variables are called measurement 
variables . 



Figure 2. Transformation Geometiy. 
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The transformation from rectangular coordinates, centered at the 
rendezvous point, to the measurement variables, y = g(x) , is 




R = [(x^+C^)^ + +(X2+C^)^] 


X = ARCTAN [ (X2+C^) / (x^+C^) ] 

x^ + Co 


3 = ARCTAN 


2i'2 


[(x^+C^)^ + (x2+C2)"^] 


(3) 


R = [(xj^+Cj^)x^ + (^3+C3)xg]/R 

X = [(x^+C^)Xg - (x2+C2)x^]/I(x^+C^)^ +(x2+C2)^] 

[(x^+C^)2+(x2+C2)^]Xg -(X3+C3) [(Xj^+C^)x^+(x2+C2)x3] 

^ [(x^+C^)^ + (X2+C2)^]^ 


The inverse transformation, x = ^-(y) is 

x^ = R cos ^ cos 3 - 

X 2 = R cos 3 sin ^ - C 2 

x^ = R sin 3 - Cg (^) 

X, = R cos X cos 3 - R3 sin 3 sin X 4- RX cos 3 sin X 
4 

x^ = R cos 3 sin X - R3 sin 3 sin X +RX cos3 cosX 

X, = R sin 3 + R3 cos 3 
o 

It was assumed that the most general set of measurements that can 
be made is range, angles X and 3, and range rate. It was assumed that 
angular rates X and 3 cannot be measured directly. 

The filter process proceeds as follows. Starting with an estimate 
Xj^y^ at time T^, and the spirit of the linear Kalman theory, a state 
estimate at time is obtained by a linear extrapolation 

through the state transition matrix for the linearized system 

^+l/k "" ^k+l/k 5k/k 


(5) 
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where 


‘*’k+l/k 
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with time to go given by 


T, ,, = T_ - T, 
k/k F k 


( 8 ) 


T = T - T 

k+l/k F k+1 
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Another method used to form the first state estimate is to 

directly integrate (fourth-order Runge-Kutta) the state estimate 

.T, 


Sk.+1/fc ' Vk 


i 


k+1 


X dt 


(9) 


With new computing equipment, this integration can be done onboard. 

After the first estimate formed, we then transfer to 

measurement variables with Equations (3) in the form 

2k+l/k “ 8 <5k+i/fc5 (nonlinear) 


( 10 ) 


The best estimate of the measurement variables at then given 

by Kalman's relation 


2k+l/k+l “ \+l/k ^-k+1 " ®2k+l/k^ 


( 11 ) 


where H is a rectangular matrix of ones and zeros that picks from 
those elements that correspond to the actual measurements, which, in 


this case, is 


H= 


1 0 
0 1 
0 0 
0 0 


0 

0 

1 

0 


0 0 
0 0 
0 0 
1 0 


0 

0 

0 

0 


( 12 ) 


is the actual measurement vector, and i® the Kalman gain 

matrix (yet to be calculated) . After filtering, transformation is 
effected back to rectangular coordinates using Equations CA) in the 
form 


5k+l/k+l ' *'(5^+i/k+l) (nonlinear) 


(13) 



-iO~ 


The Kalman gain Is calculated by 


T 


\+l/k = “kW/k « n+i/k H + \+l> 




( 14 ) 


where is a square diagonal matrix of the variances of the measure- 
ment errors, and is a matrix consisting of the transferred co- 

variance of measurement variables calculated by 


\+l/k \+k/k \/k ^ k-Hl/k 


( 15 ) 


where is the measurement variables covariance matrix at and 

is an equivalent "transition matrix" for the measurement 
variables; i.e. , 


^k+l/k ■ ’^k+l/k ^k/k 


( 16 ) 


Mehra observed that ii can be constructed by calculating 


Vl/k - ^ '^3jk/k »^k+l/k 


or 


’^k-t-l/k " ^^^k-f-l/k h+l/k ^5y^k/k 


3il> 


( 17 ) 


The matrices “^k+l/k available from the best 

/N 

estimate of state at and (^s/3^)k+l/k i® formed by using the first 

estimate at ^h® matrices OS-/3£) and (3g/3x), in columns, are 
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(M-) = 


cos A cosg 
sinA cosg 
sing 

• • 

-3 cosA sin3 -^AsinA cos3 
A cos Acos 3 -3sinA ginB 
3 cos 3 


( 



-R sin A cos 3 
R cos A cos 3 

0 , 

-R sin A cos 3 + R3 sin A sing -RA cos A cos 3 
-R cos A cos 3 - RA sinA cosg -R3 cos A sin3 
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0 

0 

0 
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sing 
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'^5 


0 

0 

0 


-R sinA cosg 
R cosA cosg 
0 


( 


^^6 


0 

0 
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-R sinA sing 
R cosg 


(18) 
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C^) 






c|i7) = 


Pj^/R 

-P2/Q^ 

-(P^P3)/(QR^) 

(P^r2-VP^) /R^ 

(P3q2-2WP^)q'^ 
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’0 1 
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where 


Pi = Xi + 

P2 = X2 + C2 

^3 " ^^3 *^3 


P 4 = ^1 


X, 


^5 “ ^5 

^6 = *3 = ^6 


Q = [P ^2 + p^ 2]2 

R = IP^2 + p^2 + p^2]^ 

A = P^P^ + P 2 P 5 
V = P^P^ + P2P5 + P3P6 
W = P^P^ - P2P^ 


( 20 ) 


All that remains is to propagate the covariance to time and this 

is done by the relation 

\+i/k+i = «-\+i/k «> \+i/k <2i) 

2.3 The Modified Guidance and Navigation Scheme 

The procedure is illustrated in the block diagram in Figure 3. 
Starting at time T^^j^ an estimate of the state is presumed available 

for evaluation. The exact state ^ is also specified at this time. The 
estimate is put into the guidance law to generate the thrusting re- 

quired over the ensuing guidance interval. The full equations of motion 
of the comet are then integrated accurately to a time and the result 

is then transformed to measurement variables and approximate noise added 
to simulate actual measurements To represent onboard computations, 

the state is propagated to time Tj^^^ through the transition matrix 

*^k+l/k integrated as shown by dotted line) . The nonlinear transfor- 
mation g(x) to measurement variables is then made to give a first esti- 
mate The_flltered estimate is then transformed non- 

A, 

linearly by Z(y) to obtain the new state estimate ^ 4 . 2 /k+i the 
process is repeated. 



Onboard 

Computation 


Comparator 






Figure 3 . Evaluation Scheme . 
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3.0 ALGORITHM DEVELOPMENT 

Having completed the reformulation in spherical coordinates , a 
second task is to reinvestigate the divergence problem. Proper choice 
of statistical quantities employed to set the filter is necessary, and 
work done to provide this proper choice is outlined in Section 3.1. 

Other approaches to improvement of system performance through 
trajectory biasing and end point control are described in Sections 
3 . 2 and 3.3. 

3.1 Investigation of Filter Divergence 

The Kalman filter theoretically produces an increasingly accurate 
estimate as additional data is processed. However, under actual 
operating conditions , error levels in the Kalman filter are often 
higher than predicted by theory. Errors can, in fact, increase con- 
tinuously although additional data is being processed. No general way 
of handling this problem is available and approximations based on ideas 
from the theory and ad hoc procedures are the rule and this is the 
approach we take. To insure that the set of measurements employed are 
sufficient to reconstruct all states, system observability is investigated 

first. 

3.1.1 System Observability 

A system is observable if all states can be reconstructed from the 
set of available measurements. For the equations 

2k+l/k " ’^k+l/k ?k/k 
-k+1 ^ -k+1 


( 22 ) 
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an augmented matrix derived by Kalman 

F = (H^ , iJiV , ^ , . . . V) 


(23) 


is formed. If the rank of the matrix F is n, where n is the order of 

the y vector, the system is observable. 

A computer program written by Bullock and Fosha^ is used to form 

T T T 

the matrix F. The matrix is normalized at each step (H , H ,...) 
so that each column has unit length. The complete matrix is renormal- 
ized so each row has unit length (The normalization procedure prevents 
overflow in large problems) . Then the square matrix FF^*^ is computed. 

If FF^ is nonsingular, the system is observable. 

Since rj; is a function of time, the matrix F is also a function of 
time. Therefore, F must be computed at each guidance interval (or any 
multiple) because a new F is present at each step, and the degree of 
observability can change. 

3.1.2 State Covariance Weighting Criteria 

In nonlinear systems, the state error covariance matrix tends to 
reduce rapidly with respect to the actual state indeterminancies. This 
can be attributed to the fact that the filter believes the first measure 
ment is highly accurate when it is not, and state estimates are biased 
incorrectly. To account for the reduction of the covariance matrix, a 

weighting procedure is employed. 

Introducing state disturbance into the system, equation (16) be- 


comes 


2k+l/k “ '^k+l/k ik/k '' ^!k 


(24) 
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where F is the disturbance transition matrix, assumed to be identity, 
and is the disturbance vector. The process {Wj^, k=0,l,...} is a 
Gaussian White sequence for which 

E [Wj^] = 0 , h = 1,2,... (25) 

Defining the positive definite matrix 

It can be shown (See, for example, Meditch^)that the state error co- 
variance matrix is given by the relation 

\+l/k " \+l/k \/k \+l/k \ 

This weighting criteria aids in eliminating the severe reduction 

of the state covariance matrix and keeps it from becoming non-positive 

definite. However, proper weighting of measurements is not guaranteed 

because Q cannot be accurately determined, and errors can still exist. 

3.1.3 Approximation of the Initial State Error 
Covariance Matrix 

In a nonlinear system, good state knowledge at time k does not 
insure good knowledge of future state because of nonlinearities in the 
dynamical equations. Hence, the Kalman gain must weight the measure- 
ments sufficiently to compensate for the errors in the one-step propa- 
gation. Measurements are not weighted enough initially when the filter 
is initialized with state error covariance matrices that are too small. 
To aleviate this problem, a design procedure for determining a suit- 
able initial state error covariance matrix is introduced. 

Assuming a priori information for the range of values of the Kalman 
gain, a suitable initial state error covariance can be found eliminating 
the incorrect weighting of measurements. The only elements that are 
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directly adjustable in the Kalman gain matrix are those which lie on the 
diagonal of the 6x4 gain matrix (to be shown later; these adjustable 
elements correspond directly to the first four diagonal elements of the 
state covariance matrix because of the special form of the observation 
matrix (discussed previously). Therefore, for given values of the 
Kalman gain, an initial state covariance matrix (four diagonal elements) 
can be found. 

There exists a one-to-one relationship between the diagonal elements 
of the Kalman gain matrix and the first four diagonal elements of the 
state covariance matrix. A change in one element of the state covariance 
matrix results in a direct change of the corresponding element in the 
Kalman gain. This can be shown by performing the matrix operations. By 
precalculation, tjj is found to be approximately an Identity matrix, so 


Equation (27) can be written 

\+l/k "" \/k \ 


where is 

\+l/k = 



(28) 


The Kalman gain relationship is given by 

•ScH-l/k ' “k+l/k 


( 29 ) 


We write out Equation (29) in explicit matrix form. 
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-1 


- 


— — 
0 


'mii+Ru ^ 


^2 


^22 


M 12 +R 22 


S 3 

= 

S3 



(30) 

K// 

6x4 

0 

1 

6x4 

0 M„+E„ 

I.* «■ 

4x4 


Notice that only the first four diagonal elements of the state covariance 

matrix have any direct effect on the Kalman gain matrix because the 

elements M^c and were eliminated in the calculations. 

5 j Ob 

The Kalman gain matrix can be reduced to a 4x4 matrix since only 
the diagonal elements are considered. The diagonal elements of the 4x6 
observation matrix correspond directly with the adjustable elements in 
the gain matrix, so H is taken to be a 4x4 identity matrix. The state 
covariance matrix is also reduced to a 4x4 matrix since only four elements 
can be directly obtained. 

Now, an initial covariance matrix can be found insuring accurate 
weights for the measurements. Rewriting Equation (29) > the Kalman gain 
is 

\+i/k ' “k+i/k <Vi/k 

Elementary matrix manipulations of Equation (31) result in 


\+l/k " " \+l/k^"^ \+l 


(32) 
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Since is approximately an identity matrix, the initial state covariance 
matrix can be calculated by 


^/k ^+l/k ^k 


(33) 


The formulation calculates only those diagonal elements in the 

state covariance matrix which correspond to the actual measurements . 

But this is sufficient because the other diagonal elements in the state 

covariance matrix do not affect the diagonal elements in the Kalman 

gain matrix. However, the other diagonal elements do affect the off- 

T 

diagonal terms in the calculation which, in turn, affects the 

fifth and sixth rows of the Kalman gain matrix. Therefore, the other 
diagonal elements must be chosen carefully. But this can only be done 
through numerical experiments. 

With this formulation, a range of values for the state covariance 
matrix corresponding to the values of the Kalman gain matrix can be cal- 
culated, and, through simulation, the desired state covariance matrix 
can be determined. However, since the unmeasured states are not direct- 
ly weighted, divergence can still occur. 


3.2 Trajectory Biasing 

In the comet rendezvous problem, the optimal control algorithm tends 
to fly the spacecraft directly toward the desired rendezvous point. If 
the rendezvous point lies directly between the comet and spacecraft, as 
illustrated in Figure 4, the measured angles change very little in the 
initial stages of flight and can cause poor estimation of the angular 
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rates (unmeasured states) . Poor estimates of the angular rates can 
cause the velocity of the spacecraft to be in error and accurate rendez- 
vous cannot be attained. To eliminate this problem, trajectory biasing 
is investigated. 


S/C 



Figure 4. Possible Rendezvous Geometry. 

Instead of flying directly to the desired rendezvous point, the 
spacecraft initially flies toward a point biased away from the target 
point. This point can be changed at each guidance step (or some mul- 
tiple) until the biased point becomes the desired rendezvous point. 
Biasing the trajectory of the spacecraft in this manner gives larger 
angular change and, consequently, better estimation of angular rates. 



With a better estimate of the unmeasured states, velocity estimates 
are more accurate, and more accurate rendezvous can be attained.' 

3.3 End point Control 

The guidance algorithm singularity leads to a "blow up" of the 
procedure at rendezvous unless appropriate steps are taken. One approach 
would be to "freeze" the commands near rendezvous, but this is not a 
good way because after the rendezvous is accomplished, it must be main- 
tained. That is, station keeping must be done. For this reason we 
looked for a modification that would transform the terminal approach 
algorithm into a station keeping algorithm while avoiding the singularity. 
One easy way to do this is simply to push time to go a predetermined 
amount as rendezvous is approached. A successive increase of time to go 
by a predetermined amount of time at each guidance step after a specified 
point will keep time to go large enough to avoid the singularity and 
automatically turns the approach algorithm into a station keeping 
algorithm. For deterministic runs and evaluations with veiry high pre- 
cision data this worked well when implemented in the form of an increase 
of one guidance time interval at each succeeding step starting at the 
time of one interval before rendezvous. However, for cases of only 
moderately accurate data, this procedure still led to thrusts above 
those easily attainable with SEP (about 10"^ g) . Several other methods 
of reducing end point thrust were considered, and n particularly simple 
one was found to work well. Instead of pushing one step, we chose an 
increase in time to go equal to 
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Where, 

A = guidance step size 

10“** = maximum SEP thrust (g’s) 

F = value of thrust in unmodified 
next interval. 

Here, the symbol 1 a| means round A to the next higher integer. Note 
that there are extensions of this technique that could be applied 
early in the terminal approach should such be required. However, this 
extension was not incorporated in the computer program as it was not 
for the cases considered. 


necessary 
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4.0 ALGORITHM EVALUATION 

The full guidance and navigation algorithm of Section 2.0 was 
programmed for simulation on an IBM 370/150 digital computer. The 
simulation package includes a double precision, fourth-order Runge- 
Kutta Integrator to provide an accurate comparison trajectory for 
evaluation of algorithm performance. A brief discussion of the program 
and user instructions are included in the Appendix. 

4.1 Development Simulations 

The starting point was chosen to be five days before rendezvous at 
a distance of 50,000 km from the target and a relative velocity of 
20,000 km/day. Measurements are made and navigation computations _iade 
each, one-tenth of a day. The rendezvous point is located 1732 km 
from the target with 1000 km displacement in each coordinate direction. 
In the first simulations, the initial state error covariance was assumed 
to be a diagonal matrix with terms of the order of 10^ in units of km 
and days. This matrix is an identity matrix in the units of 10^ km for 
distance and days for time as used in the computer program. A five 
percent error in each coordinate of the true initial state was chosen as 
the estimated position and velocity of spacecraft. 

Simulations were made and gross filter divergence was found as 
shown in Figure 5. Terminal rendezvous errors were about 2000 km and 
1,500 km/day (17.4 m/sec). Divergence began approximately four days 
from rendezvous. At this point, system observability was checked with 
the method of Section 3.1.1 to determine if the four measurements Were 




Position (km) 


Position 

Velocity 


and 

Velocity (km/day) 



Figure 5 . Navigation Errors for Initial Simulations . 

sufficient to reconstruct all six states. The observability criteria 
was applied with solutions showing complete system observability 
throughout the mission. Hence, methods for correcting divergence were 
investigated. 
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One cause of divergence was the rapid reduction of the state error 
covariance matrix. In fact, simulations show the covariance matrix 
became numerically zero in the computer near rendezvous and completely 
ignored new measurements as they were made. A standard fix is to insert 
noise into the basic system equations as discussed iri Section 3il,2. A 
constant matrix Q was added to the state error covariance matrix to 
approximate this noise. Q was arbitrarily chosen diagonal with elements 
of the order of magnitude of 10. The magnitude of the diagonal elements 
in Q are approximately the same size as those in the state error co- 
variance matrix vdien divergence started. 

Figure 6 shows a marked Improvement in the navigation errors with 
the addition of the Q matrix. Errors at rendezvous were approximately 
11.5 km and 310 km/day (3.59 m/sec) with the spacecraft a distance of 
28 km from rendezvous . 

Continuing the divergence investigation the method of Section 3.1.3 
was employed to estimate initial covariance. Assuming desired values for 
the diagonal elements of the Kalman gain matrix of about .95, an initial 
state error covariance matrix was found. Interestingly, results were 
very close to the previously assumed identity matrix and we therefore 
continued to use the identity matrix. 

The principal errors were in the velocity estimates. To get a 
better angular data; that is, data with larger changes, the trajectory 
of the spacecraft was biased away from the final target point following 
the idea of Section 3.2. The new target point was chosen as a function 
of position by the equation 

= {[x^^ + X2^ + x^^] Vl*5} + 1000 
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where is the standoff distance in the direction. The point 
changes each guidance step, and, upon a forced command, becomes the 



Figure 6, Navigation Errors with Q Matrix Added, 
desired rendezvous point at time zero. Biasing in this manner allows 
the angles to change more in the early stages of flight and thus gives 
a more accurate estimate of the angular rates. Figure 7 shows the 
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X2 



Figure 7. Comparison of Trajectories with and 

without Biasing within i/2 Day to GO* 

terminal geometry of typical biased and unbiased trajectories* A con- 
siderable Improvement in accuracy was obtained. The improvement was 
sufficient to warrant undertaking the parametric study of the effects 
of measurement accuracy. Before conducting this study, the end point 
control scheme of Section 3.3 was incorporated into the program. 
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4.2 Generation of Paramatrtc Data 

Table 1 presents nine of the data error sets employed In the 

parametric study. The choice of error in range measurement standard 

deviation, ct_, of 0.3 percent and 3.0 percent of range was made to 
R 

bracket expected errors with realistic radar equipment. Specific 
information on what these errors might be is not available and these 
values were chosen after discussions with NASA radar electronics person- 
nel. The choice of standard deviation of range rate measurements, a^, 
of 100 km/day and 1,000 km/day (1.16 m/sec and 11.6 m/sec), was based on 
these same discussions. The standard deviation of 1.0 arc seconds on 
angle measurements was also a best estimate of what could be done with 
on board equipment. This value is three times larger than the value 
used by JPL for use of ground based data reduction of spacecraft TV. 


TABLE 1. DATA SETS FOR PARAMETRIC STUDY 

(Note: 1,000 km/day - 11.6 m/sec.) 


Data Set 

Range, R 

Angles 

(arc 

X & s 
min. ) 

Range Rate, R 
(km/day) 

A 

.03156 R 

1. 

08 

10 

B 



] 

f 

100 

C 

.03 R 

1. 

0 

100 

D 





1,000 

E 





10,000 

F 

' 




100,000 

G 

.003 



10 

H 





100 

J 


' 

i 


1,000 
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5.0 RESULTS 

The parametric runs made with the data sets of Table 1 gave no 
cases of divergence even with the extreme range rate errors of sets 
E and F. And station was maintained after rendezvous for as long 
a period as run (up to 30 days after rendezvous) . The results indicate 
that the rate measurements may not be required, but this requires 
additional investigation. 

Results of four representative sets, C, D, H and J are shown in 
Figure 8. These sets bracket expected measurement errors as discussed 
in Section 4.2. In the samples, the same four sequences of random 
numbers was used to simulate the four measurements miiide (range, range- 
rate, and two angles) Other sequences gave results differing only in 
detail not accuracy levels. The plots give position and velocity 
errors only at intervals of one day because plots with the one-tenth 
day computation steps used in the simulations are cluttered and dif- 
ficult to interpret, i'he values at each day were obtained by averag- 
ing over an interval about each plotted point. 

One point is immediately obvious from Figure 8: The magnitude 

of the error in position depends almost completely on the accuracy of 
range measurements and the magnitude of the error in velocity depends 
almost completely on the accuracy of range rate measurement. For 
example, consider sets C and D. These sets have the same range measure' 
ment standard deviation (.03 R) , but different rate standard deviation 
(100 km/ day and 1000 km/day) . The position errors for these two cases 
are very nearly the same, but the velocity errors differ by a factor 



^S/7VO^ f \/^L.OCtT/ SA.JtOJe^ 
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of three to four. Similarly, sets C and tl show essentially the same 
velocity errors and a difference in position error by a factor of nine 
or ten. 

We are led to two conclusions applicable in the range of tneasure- 
ment errors considered: First, that reduction of range rate error by 

a factor of ten leads to an Improvement in velocity estimation by a 
factor of three or four and has little effect on accuracy of position 
estimation. Second, that reduction in range error by a factor of ten 
leads to an Improvement in position estimation by S factor of nine or 
ten and has little effect on accuracy of velocity estimation. The plots 
in Figure 8 show these effects add linearity. Stated another way: An 

improvement in range rate measurement accuracy produces only about one- 
third the improvement in velocity estimate while an improvement in 
range measurement accuracy leads to about the same improvement in posi- 
tion estimate. Note that the linearity of the relation for velocity 
breaks down and (fortunately) very large rate measurement errors do not 
lead to large velocity estimate errors. Range is clearly the more 
valuable data type. 

Examination of the velocity error curves of Figure 8 shows what 
might seem to be a slight divergence after the nominal rendezvous time. 
However, the reduction in accuracy is caused by the elimination of tra- 
jectory biasing shortly before zero time to go. The ensuing motion is 
a cyclic motion near the rendezvous point. This is reflected in the 
position error curves. An appropriate biasing of the trajectory and 
thrust levels can undoubtedly smooth this out. This would be a point 
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for study only later in guidance and navigation scheme development. As 
it is, the errors are not large and do not grow with time. 

The work presented here has lost much of its sense of immediacy 
because of changes in the overall NASA space program. Missions to comets 
or other deep space bodies which will require autonomous navigation and 
guidance are a long way in the future. When the time comes, the results 
here may help to give a starting point for the detailed investigations 
and development that will then be required. 
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appendix 

COMPUTER SIMULATION PROGRAM 

The computer program used is written in FORTRAN IV and used with 
the IBM 370/155. The program is a research tool, not a production 
routine. The steps in the slmulaiton and the names of the subroutines 
that carry out these steps are as follows. 

A fourth order Runge-Kutta subroutine, RUNKUT, is used to integrate 
the dynamic forces in G0FX$ over subintervals of length DT. At each 
time step (DELT) , observations are made in OBSERV and the filter is 
used to predict the state in FILTER. The Information generated is 
transferred to subroutine CYCLOT and terminal conditions are checked . 
Program sequencing and execution is controlled by subroutine CYCLE. Sub- 
routine TARGET is used to generate the comet’s position. NOISE is a 
dummy name for the functions URAND (Uniformly Distributed Random Numbers) 
and GRAND (Gaussian Distributed Random Numbers) . Ten independent noise 
channels are shared by these two functions. 

A. Subroutine Names and Descriptions 

main Reads in system data and calls CYCLE. 

CYCLE Controls sequence of operation and transfer of data 

between XT (true state) and XP (predicted state) . 

RUNKUT Fourth order Runge-Kutta integrator. Dynamics are 

provided by DOFX^ and guidance by G0FX?l. Called by 
CYCLE. Performs N integrations of step size DT at each 
call. 
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Entrles : 

EKINIT called by CYCLE 

Initialize Internal variables and read in XT 

DOFX$ - Compute contribution of dynamics to true x» J is the index 

of the components of XT* 

Entries : 

DOFX^: Compute data to be used by all components. 

DOFX: Compute each component of the true state vector. 

DXINIT: Initialize internal constants and read 

in comet data. 

SPECIAL: Calls target for comet position. 

GOEFX^; - Same as DOEFjS except XP is used as variable. 

FILTER - User supplied algorithm to calculate XP. (Basic Extended 

Kalman filter used in present listing) . 

Entries : 

FLINIT: Used to initialize arrays. 

SPECIAL: Calls MINV, CONOBS, and can call INTGR. 

OBSERV - Generates Z; may call GRAND. 

CYCLOT - Outputs data and checks for end of run. 

Entries : 

TERMIN: Check for end conditions to be satisfied. 

RECAP: If end conditions met outputs minimum 

normed distance, velocity, and associated 
times . 

CYINIT: Initialize internal consta'its . 

TARGET - Comet’s position by solution of Kepler’s equation. 

MINV - Gaussian elimination inversion routine. 

CONOBS - Determines degree of observability 

SPECIAL - Calls MTRANS, MMUL, MXINV, and NORML 
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MATRIX 


DINVER 

INTGR 

GRAND 

LSCNT 

URAND 

BLOCK DATA 


Subroutine name for group of entries . 

Entries : 

MMUL: Retforms a special matrix multiplication 

for CONOBS. 

MTRANS : Transposes matrices for CONOBS * 

MXINV t Calls DINVER 

NORML: Normalizes matrices for CONOBS. 

Matrix inversion routine using pivital condensa- 
tion for determination of determinants used in CONOBS. 

Same as RUNKUT except uses XP instead of XT. 

Generates Gaussian distributed random noise witb given 
mean (RMEAN) and standard deviation (STDDEV) » 

Noise channel number. Calls URAND. 

Generates random numbers over the interval [o,l]« 

Initializes seed numbers for URAND. 


B. 

XT 

XP 

XE 

z 

2P 

2E 

LjSL 

L{5E 

DT 


Variable Names and Definitions 

- True state vector. 

- Predicted state vector (loaded in GXINIT) . 

- Error in state. 

- True observations. 

- Predicted observations. 

- Error in observations (residuals) 

- One BYTE logical array used to control sequencing of simu- 
lator. 

- One BYTE Logical array for use by error monitor (not 
implemented) . 

- Integration stepsize (true position) . 
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DELT 

- 

Guidance update stepsize (predicted position) » 

N 

- 

DELT/DT (an integer) » 

TI 

- 

Integration start time. 

TF 

- 

Integration end time . 

ISLEN 

- 

Number of elements in state vector. 

LOLEN 

- 

Number of elements in the observation vector. 

C 

- 

Rendezvous stand-off distance i 

A 

- 

Target semi-major axis. 

m 

- 

Target mean motion. 

EPS 

- 

Target eccentricity . 

EO 

- 

Target eccentric anomaly. 

TSTAR 


Guidance initiation time. 


C. Input Data 


Card Number 

Contents 

Format 

1 

Title Card 

SA8 

2 

Run time logical 
flags (L^L) 

80L1 

3 

Run time error 
flags (Lj^E) 

80L1 

4 

N, ISLEN, lOLEN 

813 

5 

TI, TF, DT, DELT 

8F10.0 

6 

XT (Initial conditions) 

8F10.0 

7 

A, RN, EPS, EO, TSTAR 

8F10.0 

8 

C 

8F10.0 
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D. Subroutine Initialization Entries 


Subroutines and 
Line Numbers 


Entry 


GX INIT(J) 

38,39,40 

Initial position error 

41,42,43 

Initial position error 

FLINIT 

238-243 

Initial state error 
covariance matrix 

OVINIT 

90 

Staiidard deviation of angular 
measurement error (EANG) 

91, 92 

Fix logical if statements for 
RMIN and RDMIN 

OBSERV 

30-33 

Mean, standard deviation and 
noise channel. 

24-27 

Range, angles, and range rate 
errors (RFRAF) 



PROGRAM LISTING 
MAIN 


IMPLICIT REAL+8 lA-H.O-Zl 
LOGICAL*! L$L,L$Ef LMON,LF,LT 

C0MM0N/V$«BLE/XT(6),XP<6) tXE<6) ,Z(6I,ZP(6) ,ZE(6) 
COMMON/T$MER/DELTfDT,TIME,TI,TF,M, ISLEN.IOLEN 
C0MM0N/SY$TEM/L$L(40),L$EI 10) 
C0MM0N/M$NITR/LM0NC20) 

C0MM0N/N0I$E/ IRANI 10) ,DG( 10),RFRAF<6) 
C0MM0N/0FFSET/CI3) 

C0MM0N/M00CH/T6(4,4) 

DIMENSION Hl4,6)fRNI4,4) 

DATA LFtLT/F,T/ 
b02 FORMAT! 80L1) 

503 F0RMATI5A9) 

504 FORMATI8I3) 

505 FORMAT! 8FI0.0) 

602 FORMAT! IH0*80L1 ) 

603 FORMAT! 1H0,5A8) 

604 FORMAT! 1H0»» INPUT CARD LIST*) 

606 FORMAT! IH ,////) 

607 FORMAT! IH0#8I5) 

608 FORMAT! IH0,1P6D12. 5) 

WRITE!6,604) 

READ!5f 503)TITLE 

WRITE!6,603)TITLE 

READ!5,502)L1L 

WRITE !6,602)L$L 

READ!5f 502 )L$e 

WRITE!6t602)LlE 

READ!5,504)N,I5LEN, lOLEN 

WRITE!6,60/)N, ISLEN* lOLEN 

READ!5,505)TI,TF,DT,DELT 

WRIT£!6,608)TI ,TF,DT,DELT 

WRITE16t606) 

CALL CYCLE 

STOP 

END 

SUBROUTINE CYCLE 
IMPLICIT REAL*8 !A-H,0-Z) 

LOGICAL*! L$L,L$EtLMONfLF,LT 

C0MMUN/V$RBLE/XT(6) ,XP!6) »XE!6),Z(6),ZP!6),ZE16) 
COMMON/ T$MER/DELT,DT,TI ME, T I, TF »N, ISLEN, TOLEN 
C0MM0N/SY$TEM/L$L!40) ,L$E! !0) 
COMMON/MtNITR/LMONI20) 

COMMON/NOI tE/IRAN! ! 0 ) , DG( ! 0 ) , RFRAF ! 6 ) 
C0MM0N/0FFSET/CI3) 

C0MM0N/M0rjCH/T6!4,4) 

DATA LF,LT/F,T/ 

DIMENSION B!4,6) ,RN!4,A) 

c initialize subroutines **** 

CALL RKINIT 
DUM=DXINIT( !) 


-40 
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DUM=GXINIT( 1) 

DUM=URINIT( 1 1 
CALL CYINIT 
CALL FLINIT 
CALL OVINIT 
CALL INTINI 
OUM=OXININ( 1 ) 

OUM=GXININ( 1 ) 

C COMPUTE TRAJECTORIES 

1 CALL RUNKUT 
IF<L$L(2) )GO TO 2 
CALL OBSERV 

CALL FILTER 
GO TO 3 

2 DO 4 I=1,ISLEN 

4 ;<P( I )=XT( n 

C OUTPUT CYCLE DATA **** 

3 CALL CYCLOT 

C MONITOR SECTION **** 

CALL TERMIN 
IF(L$L(2))G0 TO 5 
IF(L$L(3) )GO TO 5 
IF(L$L(6> )GO TO I 
GO TO 7 

5 DO 6 I=l,ISLEN 

6 XP( I )=XT{ I ) 

IFa$L(6))G0 TO I 

C OUTPUT SECTION **** 

7 CALL RECAP 
RETURN 
END 

SUBROUTINE RUNKUT 
IMPLICIT REAL*8 (A-HtO-Z) 

LOGICAL*! L$L,L$6,LMONtLF,LT 
LOGICAL*! LS!,LS2 

COMMON/VtRBLE/XTI 6) , XP ( 6) t XE ( 6 ) t Z ( 6 ) f ZP ( 6 ) » Z£ ( 6 J 
COMMaN/TtMER/DELT,DT,TIME,TI »TF,N, ISLEN» lOLEN 
COMMON/SY$TEM/L$L(40 J tL$E( !0) 
COMMON/M$NITR/LMON(20) 

COMMON/NOI tE /IRANI !0) ,DG( !0) fRFRAF(6) 
COMMON./OFFSET/C ( 3) 

DIMENSION XINT(6),SUM<6» 

DATA LF,LT/F,T/ 

LtL(!2)=LT 

YS=DSQRT( XP( U**2+XP( 2 ) **2+XP{ 3) **2> 

C( i )=1.0D-02+(YS/!.5D0) 

IF{TIME.GT.38.4) C(!)=1.0D-02 
WRI TE(6, 7*50»C 

75 0 FORMAT! !H0,7X, *C1' , IIX, • C2 • , ! ! X , • C 3 * /4X , ! P3D ! 2 . 5 / ) 
DO ! ICYCLE=!,N 
DO 33 I=1,1SLEN 
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33 SUMCn = O.ODO 
L$L(10)=LT 
DO 10 ll=lt^ 

LS1=I1.EQ.2. QR.Ii.EQ. 3 
LS2=I l.EO.4 

L$Lm)*I i.EQ.3 

F*F1 

FS=F5 

IF(LSUF=F2 
IF(LSnFS=F3 
IF(LS2)FS=F4 
TS=TIME+OT*FS 
DO 20 I=ltISLEN 
20 XlfMKn^XTUKFS^XlMTd) 

DO 31 I=l*MPl 
J=I-l 

IF(J.GT.0S GO TO 2 
DUM=D0FXS « JfTS) 

OUM=GOFX$( JtTS> 

L$U 12)=LF 
GO TO 31 

2 XINT( J)=DT*(D0FX( J»+GOFX(JM 

SUM( J ) = SUM( J)-*-F*XlNTC J) 

31 CONTINUE 

L$L(10)=LF 

10 CONTINUE 
time=time+ot 

DO ll I=1»ISLEN 

11 xT( n=xT ( n-*-suM( n 
1 CONTINUE 

RETURN 

ENTRY RKINIT 

READ! 5t 501) (XT ( I) f I^^ltlSLEN) 
WRITE(6,601) (XTl I ) * I = 1*1 SEEN) 
501 FORMAT! 8F10.0) 

601 FORMAT! IHO, 1P8D12. 5) 
F1=1.0D0/6.0D0 
F2=2«0D0*F1 
F3=l.OD0/2.0D0 


32 


F4=1.0D0 

F5=0.0D0 

TIME=T1 

NPl=I Sl.EN+1 

DO 32 I=1*ISLEN 

XIlMTm=XT!l) 


[TURN 

JD 

JNCTIDN DnFX$(J*TS) 

^PLICIT REALMS! A-HfO-Z ) 

IGICAL*! L4L *L$E » LMON* LF* LT 

3MM0N/V$RBLE/XT! 6) , XP ! 6) , XE ! 6 ) * Z ( 6 ) * ZP ! 6 > i 


ZE !6) 
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COMMON/ T$MER/DELT,DT, TIME tTI ,TF,N, I SLEN.IOLEN 
C0MMON/SY$TEM/LSL(40),L$E( 10) 

C0MM0N/M$NITR/LMON(20) 

COMMON/NOISE/ 1 RANI 10) tOGI 10) tRFRAF(6) 

COMMON/ VAR 1/A, RNf EPS, EOfTSTAR,DET 
C0MM0N/0FFSET/C(3) 

DIMENSION DI3),SI3) 

DATA LF,LT/F,T/ 

99601 FORMAT! IHO, 14, • IMPROPER INDEX ♦DOFXM 
IFI .NOT.LSLIll) )CALL TARGET (S,TS) 

52=0. ODO 
D2=0.0D0 
DO 1 1=1,3 

D( I ) = S( I )+C( I )+XT( I ) 

D2=D2>DI I ) **2 
1 S2=S2+S( I )**2 

DN=DSQRT(D2) 

SN=DSQRT( S2) 

RATI=GM/( SN*SN*SN) 

RAT2=(SN/DN)**3 

DOFXJ=O.ODO 

IF(TF-TIME.GT.OELT)RETUKN 
IFI .NOT.LSLI 10) )RETURN 
WRITEI6,602)TIME,XT 

602 FORMATI IHO, »END STATE * ,2X, • T1ME=» ,F10. 3/lH ,1P6D12.5) 
RETURN 

ENTRY DOFXIJ) 

GO TO 199999,99999,99999,99998,99998,99998) ,J 

WR1TEI6, 99601) J 

DOFX=0.000 

L$EI2 )=LT 

RETURN 

99999 D0FX=XTtJ-f3) 

RETURN 

99998 D0FX = RAn^(SIJ-3)-0IJ“3M‘RAT2) 

RETURN 

ENTRY DXiNiTIJ) 

READ(5,501 )A,RN,EPS,EQ,TSTAR 
EQ=RN«TSTAR 

WRiT£I6B601)A,RN,EPS,E0,TSTAR 
501 FORMATI 8F10.0) 

601 FORMAT! IHO, 1P8D12. 5) 

READ! 5,501 )C 

WRITei 6 s» 60 nC 

GM=9.90549D05 

DET=l.0D“3 

DXINIT=0.0D0 

RETURN 

END 

FUNCTION GOFX$U,TS) 

IMPLICIT REAL<‘8IA-H,0-Z) 
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LOGICAL*! LSLf L$EfLMON»LF»LT 

C0MM0N/V$RBLE/XTt6),XP<6),XE(6),Z<6lpZP(6),ZEl6l 
COMMON/T$MER/OELTfOTtTIME#TI*TF,N, ISLEf^t lOLEN 
C0MM0N/SY$TEH/L$L(40) ,L$E< lO) 

COMMON/M$NITR/LMOf^( 20) 

COMMON/NOI$E/IRAlYUO)tDG( 10),RFRAF«6) 
C0MM0N/F0RCE/FI3) 

C0MM0N/0FFSET/C(3) 

CQMM0N/XPR0P/XPNEW(6) 

DATA LF.LT/F.T/ 

DIMENSION XERRI6) 

99601 FORMATIIHO, 14, 'IMPROPER INDEX *GOFX») 

IF(L$L( 12) )TIM1=TS 

TAUO=TF-TIMl 

TAU=TF-TS 

TRAT1=( 6,ODO/TAUO«*2 )♦( 1,0D0-2.0D04( TAU/TAUO) ) 
TRAT2=( 2.0D0/TAUO)*! I .0D0-3.0D0»I TAU/TAUO) ) 
GOFX$=O.ODO 
RETURN 

ENTRY GOFX(J) 

GO TO ( 99999,99999,99999,99998,99998,99998 ) ,J 

WRITEI6, 99601) J 

L$E(3)=LT 

GOFX=O.ODO 

RETURN 

99999 GOFX=O.ODO 
RETURN 

99998 F( J-3) = TRAT1*XP(J-3)>TRAT2*XP( J) 

GOFX=F( J-3) 

RETURN 

ENTRY GXINIT(J) 

WRITE(6,500)(XT(I),I=1,6) 
bOO FORMAT! 1HU3X, 'XTRUE I NI TI ALL Y» /4X , 1P6D12 . 5/ ) 
XERR( l)=2.0D-02 
XERR(2)=1.5D-02 
XERR( 3) =1.0D-02 
XERR(4)=7.0D-03 
XERR(5)=5.0D-03 
XERR(6)=4.0D-04 
WRITE(6,501) (XERR( I ), 1=1,6) 

501 FORMAT! 1H0,3X, 'INITIAL ERROR ON X'/4X, 1P6D12. 5/) 
DO 90001 I=1,ISLEN 
90001 XP! n=XT! I )<-XERR! I ) 

WRITE!6i,7T6M XP! I ) ,I = 1,ISLEN) 

776 FORMAT! 1H0,3X, 'XHAT INI riALLY'/4X, 1P6D12.5/) 

DO 25 1=1,6 
25 XPNEW! I )=XP! 1 ) 

GXINIT=O.ODO 

RETURN 

END 

SUBROUTINE FILTER 
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IMPLICIT REAL*8 ( A-H,0-Z ) 

REAL G,BO,PSI 

LOGICAL^l L$Lf LSEtLMONf LF,LT 
C0MM0N/V$RBLE/XT(6),XP(6),XE(6) ,Z(6),ZP(6) ,ZE(6) 
COMMON/T*MER/DELT,OT,TlME,TI,TF,N, ISLEN,I□LE^^ 
COMMQN/SY$TEM/L$LUO) ,L$E{ 10) 

COMMON/M$MlTR/LMOrj( 20 ) 

COMMON/ NO I JE/IRAN( 10) rOG( 10 I tRFRAF(6) 

C0MM0N/M00CH/T6(4 4) 

C0MM0N/XPR0P/XPNEW(6) 

C0MM0N/YTRUE/YTRUE(6) 

DIMENSION T1(6),YL(6),Y(6) ,T2I6,6),T3(6,6) ,T4(6,6) ,QP( 

^6 » 6 J f 

tr5(6,6) ,PHI(6,6) 

dimension 0Y(6) ,QEXT( 6f 5) ,RU( 1 ) ,DDY( 6) 

DIMENSION BQ(4f 6) fPSn6,6) 

DIMENSION G(6,l) 

DIMENSION LL(6),MM(6) 

DATA LF,LT/F,T/ 

TAUKl=TF-TIME 
TAUK = TAUKUOELT 
RAT=TAUK1/TAUK 
RAT2=RAT4RAT 


RAT3=RAT24RAT 

A=3.0D0^RAT2-2.0D0*RAT3 

DIF=RAT-RAT2 

F=TAUK1*DIF 

D=3.OD0*RAT2-2.ODO*RAT 
E=-6.0D0#DIF/TAUK 
DO 31 1=1,3 
PHin.I )=A 
PHK I,I^3)=F 
PHni+3,I)=e 
PHI ( 1+3, I+3)=D 
31 CONTINUE 
C PREDICT X STATE 

IF(L$L(9) ) GO TO 8000 
DO 10 1=1 , ISLEN 
T1 ( I )=0.0D0 
DO 10 J = l, ISLEN 

10 ri( n=Ti(i H-PHi(i,j)*xp(j) 

GO TO 8002 

8000 CONTINUE 
CALL INTGR 

DO 8001 I=l,ISLEN 

8001 Tl(n=XP(I) 

3002 CONTINUE 

C TRANSFORM TO Y SYSTEM 
P1 = T1(1 )+C( 1) 
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P2=T1(2I*CI2) 

P3=Tl(3)+C( 3J 

Q*DSQRT(P1**2^P2**2I 

R*OSQRT(Pl**2-»>P2**2^P3»<'2) 

AU=P1*TI(4KP2*TU5) 

V=AU+P3*T1<6) 

W=( tPU^(TU5) ) )-((P2»»(TU4) ) I 
Yl l)=R 

Y(2)=DATAN2(P2#P1I 

Y<3)=DATAN2(P3fQ) 

YC4)=V/R 

Y(5)sW/(Q**2l 

Y(6)=(Q**2*Tl(6)~AU*P3l/(li*R<'*2» 

CL=DCOS( Y(2) ) 

CB=DC05( YC 3) ) 

SL=0SIM(Y(2) ) 

SB=DSIN(Y( 3) ) 

C COMPUTE LEFT PARTIAL DERIVATIVE 
T2(1,U=CL»CB 
T2(2, U=SL*CB 
T2{ 3, n=SB 

T2(4, l)=-Y(6)*CL*SB-Y(5)^SL*CB 
T2(5, l)*Y( 5)*CL*CB-YC6H'SL*SB 
T2(6,1)=Y(6)*CB 
T2( 1,2)=-Y( 1)*SL^CB 
T2(2,2)=Y( 1)*CL*CB 

T2(4,2)=-Y(4)«‘SL*CB4Ym*Y(6»*SB*SL~YU)*Y(5)*CL*CB 
T2( 5,2)=YI 4)*CL»CB-Y( 1 )«Y( 5)*SL>0«CB-YI U«Yt 6)»CL^SB 
T2( l,3)=-Y< 1 )*CL*SB 
T2(2.3)=-Y( 1 )*SL*SB 
T2(3,3)=Y( 1)*CB 

T2(4, 3)=-Y(4)#CL^SB~Y( 1)*Y(6)*CL*C0+Y(1)*Y(5I*SL*SB 

T2(5,3)=~Y(4)*SL*Sb-Y( 1) ♦Yl 5 ) ♦CL + SB-YIU ♦Y U) ♦SL^CB 

T2(6,3)=YI4)*CB-Y( 1)*Y(6)*SB 

r2(4,4)= CB^CL 

T2(5,4)=SL«CB 

T2(6,4)=SB 

T2(4,5)=-YI1 )«SL*CB 

T2( 5, 5)=YI U*CL*CB 

T2(4,6)=“Y( IH'CL+SB 

T2{5,6)=-YU )*SL*SB 

T2(6,6)=YI 1)*CB 

C COMPUTE RIGHT PARTIAL DERIVATIVE 
T3( 1, 1)=P1/R 
T3( 1,2)=P2/R 
T3I I, 3)=P3/R 
T3(2, 1)=-P2/IQ**2) 

T3(2,2)=P1/(Q**2) 

T3I 3, l)=-Pl«P3/(0*R**2) 

T3(3,2)=>P2<'P3/IQ+R**2) 

T3(3,3)=Q/ (R**2) 
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T3(^f 1)*<T1(^)*R**2-V*P1)/<R**3) 
T3(4,2) = (R**2*TU5)-V*P2)/<R**3) 

T3(4,3)=I Tl( 6)*R**2-V*P3)/(R^*3) 


T3(4,4)=P1/R 

T3(4,5)=P2/R 

T3(^t6i=P3/R 

T3(5f 1)=(T1(5)/CQ*^2) )-t2.*W»Pl)/CQ**^) 
T3C5f2)=(-Tl(4»/(0**2) )-(2.*W*P2)/(Q**A) 

T3( 5,A)=-P2/(Q**2) 

l)=*(Pl*TUr)-P3*ri(4) )/(Q*R**2)-2.*Q*Pl*Tl(6)/(R* 




«2 ♦au*P 1*P3/(Q>«'R**^)^P1^R3*AU/(Q**3=*R**2) 

T3tt.2) = IP2»Tl(61-P3*Tl(5n/(0*R»*2»-2.*Q»P2*Tn6 l/IR* 


♦ ) + 


o AAiiJ|cp2*P3/ (Q*R**^)'*'AU*P2 ’*‘P 3/(0**3*R**2) 

T3(tr3)=-AU/(Q«R»»2)-2.*P3Ml(6)*0/(R**'.»*2..AU»P3*« 


t(Q*R**4) 

I^3(6,4)=-PI*P3/(Q<'R**2) 
T3( 6.5)=-P2*P3/(Q»R**2) 
T3(6,6)=Q/<R**2» 


C COMPUTE PSI 

00 12 I=lfISLEN 

DO 12 J=l,lSLEN 

T4( I , J)=0. 300 
DO 12 K = lf ISLEN 

DO 12 L=lf ISLEN i\ 

12 T4( I,J)=T4(I,J)-H2( I ,K)*PH1 (K,L)»T3(L,J> 

DO 9990 1=1*6 
DO 9990 J=l*6 
G( l,l) = 0.0 

9990 PS I ( I I J ) =T^( I * J ) 

DO 9991 1=1*^ 

DO 9991 J=l*6 

999 1 BQ(I»J)=B(If3) 

C PREDICT COVARIANCE 
DO 13 1 = 1* ISLEN 
DO 13 J=l* I SLEN 
r5(i*j)=Qpn*J) 

DO 13 K=1 » I SLEN 

DO 13 L = 1 * ISLEN , , . » 

13 T5( I*J)=T5n*J)+T4( I , K ) «RM ( K , L ) *T4( J , L ) 
DY( 1)=“2. CD-05 
DY( 2)=-4. lD-05 
DY( 3)=-3. 30-05 
DY(4)=1. 50-05 
DY( 51=9.210-05 
0Y(6)=4. lD-05 
DO 600 1=1*6 
DO 600 J=l»6 

600 QEXT(I*J)=DY(l)*DY(J) 
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DO 980 I=lf6 

980 OEXTd, n«Q6XT( I, n*l-0D6 
00 601 l=l#6 
DO 601 J»lf6 

601 T5Uf J) = T5(I»J)>QEXT( I, Jl 
C COMPUTE THE FILTER 
DO 14 I*ifIOLEN 
DO 14 J-ldOLEN 
T6n,JI»RNn,JI 
DO 14 K-lf ISLEN 
DO 14 L=lt ISLEN 

14 T6( I,JI=T6( I,K)*T5(K,LI^B<J,U 
CALL MINV(T6,I0LEN,16»LLfMMfDI 

DO 15 I=ltISLEN 
DO 15 J*1,I0LEN 
FILT( I* J)=0.0D0 
00 15 K=ltISLEN 
DO 15 L*ltIOLEN 

15 FILT( I»J)=FILT( I#J)+T5(IfK)*B(LfK)*T6(L»J> 
C COMPUTE PREDICTED OBSERVATIONS 

DO 16 I=lfIOLEN 
ZP( I )=0.0D0 
DO 16 J = l, ISLEN 

16 ZP( n=ZPM )+B( I,J)*V(J» 

C COMPUTE THE ERROR IN OBSERVATIONS 
DO 17 I=l,IDLEN 

17 ZE( n=z( n-ZP( I) 

C UPDATE Y 

DO 18 1 = 1, ISLEN 
Tin )=Y( I I 
DO 18 J=1,I0LEN 

18 TUI )=T1( I M^FILTl I ,J)»ZE( J) 

C UPDATE COVARIANCE 

DO 19 1=1, ISLEN 
DO 19 J=l, ISLEN 
T4(I,J)=O.ODO 
DO 19 K=l , lOLEN 

19 T4( I,J)=T4( I ,JI+FILT( I,K>*B(K, J) 

DO 20 1=1, ISLEN 

DO 20 J = l, ISLEN 
T4( I,J)=-T4n,J) 

IF( I.E0.J)T4(I,J)=T4(I ,J)+1.0DO 

20 CONTINUE 

DO 21 1=1, ISLEN 
DO 21 J=l, I SLEN 
RM( I, J)=0.0D0 
DO 21 K=l, ISLEN 

21 RM( I , J)=RM(I,J)+T4( I ,KI*T5(K,J) 

99 CONTINUE 

C SAVE Y(K+1,K+1I 

DO 22 1=1, ISLEN 
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22 YL( 1 )-Tl( I ) 

CALL C0N0BS(6tlt4tPSI t6tBQ> 

C CONVERT TO X 

CL=DCOS(YL<2» ) 

CB=DCOS( YL(3H 
SL=OSIN(YL(2>) 

SB=DSIN( YLI3) » 

XP(l»*YL(U*CB*CL-C«U 
XPI2)aYL( n*CB*SL-C(2» 

XP(3)= YLC n*SB-Ct3) 

XP(4)sYL(4l*CB*CL-YLm<'VL(6»*SB*CL-YL(U*YL(5|»CB*SL 
XP( 5 »=YH4I*C8*SL-YL( U 4YL ( 6 1 ♦SB*SL4-YL ( II *YL ( 5 ) *CB»CL 
XP(6)*YL(4)4SB'fYLUl*YL(6)*CB 
WR1TE(6*912MXPU)»J»1»6| 

912 FORMAT! IHO , 3Xf • XHAT • /4X f IP6D12 . 5/ I 
DO 913 I=lt6 

913 XPNEW( I l = XP( 1 1 
RETURN 

ENTRY FLINIT 

* ENTRY 
C ZERO ARRAYS 

DO 101 I=lfISLEN 
00 102 J=lfISLEN 
T2( I »J)=O.ODO 
T3( I f J)=O.ODO 
RM( I t J)*0«0D0 
PHK ItJ)=0.0D0 
102 QPUtJl'O.ODO 

DO 101 J=1#I0LEN 
101 FILT( I f JI=0.0D0 
C INITIALIZE VARIABLES 

YL( 1 1 =DSQRT( CXP 1 1 ) + C( 1 ) H'*2+ ( XP I 2 1 ♦C ( 2 1 ) ♦»2+ ( XP ( 3 ) +C ( 3 
)♦♦2) 

YL( 2)=DATAN2( ( XP( 2 I +C ( 2 I) » ( XP (1 ) +C U 1 1 I 
ARG2=DSQRT( (XPI 1)+C( in**2 + (XP(2)+C(2))**2) 

YL(3 l=DATAN2( ( XP ( 3 UC ( 3 1) t ARG2 I 

YL(4I = ! ( XP( 1)4-C( 1) )♦XP{4)^■( XP(2)+C(2I I ♦XPI 5 1 ♦ I XP ( 3 ) ^C ( 

♦ 3) I^XPIB) ) 
t/YL(l) 

YL(5) = ( (XP(l)+C(in*XP(5)-<XP(2)+C(2))*XP(4) )/ 

$( (XP( 1)+C( II l♦♦2■»•( XP{2I+C(2) 1 4*2 I 
YL(6) = n (XP( 1I+C( 1 1 |♦♦2 + (XP(2I•^C(2) |**2 1 *XPI 6 1 - ( XP ( 3 )♦ 
*C(3) )♦ 

$( (XP( 1I+C( II l♦XP(4) + {XP(2I♦C<2) )♦XP(5m/(YL( 1I**2*ARG 
*21 

RMI 1, ll^l.ODO 
RM(2,2I=1.0D0 
RM( 3,31=1.000 
RMI4, 41=1. 000 
RM(5, 51=1. 000 
RMI 6,61=1.000 
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WRI re(6,652) 

652 FORMAT! 1H0,3X*MNITIAL COVARIANCE MATRIX* J 
DO 650 I=lt6 

650 WRITE(6t65I) (RM(I, J) • J=lf 6) 

651 FORMAT! IH0.4X, IP6D12. 5) 

RETURN 

END 

SUBROUTINE OBSERV 
IMPLICIT REAL*8! A-HfO-Z) 

LOGICAL*! L$LfL$Ef LMON»LF,LT 

C0MM0N/VSRBLE/XT(6)tXP!6) ,XE!6I ,Z!6),ZP!6I ,ZE!6) 

common/t$mer/oelt,dt,time,ti,tf,n,islen,iulen 
COMMON/SY$TEM/L$L! 40) fL$E( 10) 

COMMON/M$NITR/LMON!20) 

COMMON/NOISE/IRAN! 10) »DG! 10) tRFRAF!6) 
CQMMON/KALMAN/CRUD! 66) fRN!4,4)t8!4,6) 
COMMON/OFFSET/C!3) 

COMMON/ YTRUE/YTRUE! 6) 

DIMENSION ZT!6) 

DATA LF,LT/F,T/ 

C OBSERVATIONS 

C RFRAF!l TO 4) = STDDEV FOR ZT!1 TO 4) RESPECTIVELY 

YTRUE! 1)=DSQRT! ( XT ! 1 ) *C ! 1 ) ) **2* ! XT ! 2 ) *C ! 2 ) ) **2^^ ( XT ! 3 ) + 
*C!3) )**2) 

RANGE=YTRUE! 1 ) 

YTRUE! 2 )=DATAN2 ! ! XT ! 2 )*C! 2 ) ) t ! XT! 1 ) +C! 1 ) ) ) 
ARG1=DSQRT!!XT! 1)*C! 1) )**2+!XT!2)+C!2) )**2) 

YTRUE!3 )=DATAN2 ! ! XT! 3)+C! 3) ) tARGl ) 

YTRUE !4)=I!XT!1)+C!1))*XT!4)*!XT!2)+C!2))*XT!5H-!XT!3) 
♦♦C! 3) )*XT! 6 
%) )/YTRJE! I ) 

YTRUE!5)=! ! XT ! 1 ) +C ! 1 ) ) *XT ! 5 ) - ! XT ! 2 ) ^C! 2 ) ) *XT ! 4 ) )/! ARGl 
) 

YTRUE (6)=!!ARGl**2)*XT!6)-!XT!3)+C!3))*!!XT!l)+C!l))*X 

*T !4) 

t+!XT!2) +C!2) )*XT!5) ) )/! YTRUE ! 1 ) **2*ARG1 ) 

RFRAF! 1 )=DABS! YTRUE! 1 ) *3. 00-03) 

RFRAF!2 )=EANG 

RFRAF ! 3 )=EANG 

RFRAF!4)=1.0D-02 

DG! 1 )=GRAND! O.ODO,RFRAF! 1 ) f 5) 

DG!2)=GRAND!0.0D0f RFRAF!2) 9 2 ) 
DG!3)=GRANr)!0.0D0fRFRAF!3) t3) 
DG!4)=GRAND!0.0D0,RFRAF!4) ,8) 

IF(RANGE.GT.RMIN)GO TO 2 

IF!RANGE.LT.RMIN. AND. RANGE. GT.RDMIN)GO TO 3 
IF!RANGE.LT.RDMIN. AND. RANGE. GT.RMIN) GO TO 6 
IF!RANGE.LT.ROMIN)GO TO 8 
2 WRITE(6,1) !OG!LSCNT) ,LSCNT=2,3) 

I0LEN=2 
GO TO 4 
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3 I0LEN=3 

WRITE(6,1) (OG(tSCNn,LSCNT=l,IOLEN) 
1 FORMATS ///,10X» 'MOISES5X.1P4D12.5) 
GO TO 4 

6 WRITE(6#l) (DG(LSCNT) ,LSCNT*2f4» 
IOLEN=3 
GO TO 4 
B I0LEN=4 

WRITE(6.n(DG(LSCf^T),LSCNT*l,I0LEN> 

4 CONTINUE 

DO 38 I*li lOLEN 
DO 32 J-1#I0LEN 

32 RNdf J)=0.0D0 
DO 38 J=l,ISLEN 

38 8(ItJ)=0.0D0 

IF( IOLEN.EQ.2)GO TO 34 
tF(L$L(20).AN0*I0lEN.EQ.3)G0 TO 37 
DO 33 I=1#I0LEN 

33 B{I,I)=UODO 
GO TO 36 

37 B(lt2)=l.0D0 
B(2t3)»I«0D0 
B(3,4)=1.000 
DO 39 1=1,3 

39 RN(I,n=RFRAFn>l)*42 
GO TO 35 

34 B(L,2)=1.0D0 
B(2,3)=1.0D0 

RN( I, U=RFRAF(2)**2 
RN(2,2)=RFRAF< 3)#*2 
GO TO 35 

36 DO 30 I=l,IOLEN 
30 RNII, I )=RFRAFiI )4*2 

35 CONTINUE 

DO 41 I=l,IOLEN 
ZTm=0.0D0 
DO 41 J=l,ISLEN 
41 ZT( I )=ZT{ I )+B( I ,J»*YTRUE( J) 

DO 43 I=1,I0LEN 
43 Z(1 ) = ZTC I UDG( 1 1 
RETURN 

ENTRY OVINIT 
* ENTRY 

A=0.0D0 

EANG=2.906D-04 

RMIN=1.0D20 

R0MIN=1.0D20 

RETURN 

END 

FUNCTION GRAND(RMEAN,STDDEV, ISLCT) 
IMPLICIT REAL*8U-H,0-Z) 
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PURPOSE 

COMPUTES A NORMALLV DISTRIBUTED RANDOM NUMBER WtTH A G 
«IVEN 

C MEAN AND STANDARD DEVIATION 
A*O.ODO 
DO 50 I=lfl2 
50 A‘A4-URAN0(ISLCT) 

GRAND* C A-6 • 000 ) *STODE V>RME AN 

RETURN 

END 

FUNCTION URANDdSLCTI 
IMPLICIT REAL*8«A-H,0-Z) 

COMMON/NOI$E/IRANUO»#OG( 10) tRFRAF(6l 
IY=IRAN( ISLCT)*65539 
IF( IY)5#6,6 

5 IY=IY+21474B36A7F1 

6 URAND*DFLOAT( IY)*4.6566l3D-10 
IRANI ISLCT)=IY 

RETURN 

ENTRY URINITI ISLCT) 

A ENTRY 

URAND=0.0D0 
URINIT*0.000 
RETURN 
END 

BLOCK DATA 

IMPLICIT REAL*8 (A->H,0-Z) 

COMMON/NOI$E/IRAN( 10) tDGI 10) #RFRAF(6) 

DATA I RAN/69800661, 542 1 8059, 51 070625* 15239339 f 75892237 

♦* 

*10418327,81767867,59847821,52031357,26256073/ 

END 

SUBROUTINE CYCLOT 
IMPLICIT REAL*8(A-H,0-Z) 

REAL GUTL 

LOGICAL*! L$L,LiE,LMON,LF,LT 

C0MM0N/V$RBLE/XT(6) ,XPI6) , XE I 6 ) , Z I 6) ,ZP( 6) * ZE < 6 ) 
C0MM0N/T$MER/DELT,DT,TIME,TI,TF,N, ISLEN#I0LEN 
C0MM0N/SY$TEM/L*L(40) ,L3EI 10) 

C0MM0N/MiNITR/LM0N(20) 

C0MM0N/N0I$E/IRAN(10),DG( lO),RFRAF(6) 

COMMON/ KALMAN/ P <6,6 ) ,FILT( 6,4) ,U( 6), RN (4,4) ,BI 4,6) 
C0MM0N/F0RCE/FI3) 

C0MM0N/0FFSET/C(3) 

C0MM0N/M00CH/T6(4,4) 

DATA LF,LT/F,T/ 

601 FORMAT! IH ,3X,*TRUE STATE VECT0RV4X,1P6012.5) 

603 FORMATIIH , ‘TIME** , IP012,5) 

604 FORMATIIH ,*NORMEO D I STANCE* * , 1PD12. 5, 3X, • NORMEO VELOC 
*ITY=», 

11PD12.5,3X,»N0RMED FORCE** , IPD12.5) 
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605 FORMATt IHO, REi^DEZVOUS $4iM 

606 FORMATdHOt'MINIMUM r^ORMED DI STAf 4 CE= • , IPDl 2- 5, 3X ♦ • AT T 
*IME=* t 

1 IPD12.5) 

607 FORMAT! 1H0,*M1MIMUM FORMED VELOCITY =• , IP012. 5*3X, • AT 
♦TIME*'* 

i IPD12.5I 
600 FORMAT!////) 

609 FORMAT!1HO#'**** OUT OF TIME **♦•> 

610 FORMAT! IHO, 'FORCE VECTOR'#/#lH ,IP3012.5) 

611 FORMAT! IH0»3X, 'PREDICTED STATE VECTOR' /4X, 1P6D12. 5 ) 

612 FORMAT! lH0*3Xt 'ERROR IN STATE VECTOR'/'^Xt IP6D12.5) 

613 FORMAT! IH0*3X,' TRUE OBSERVATIONS • /4X , IP601 2 . 5 ) 

614 FORMAT! IHO, 3Xt 'PREDICTED 0BSERVATI0NS'/4X, IP6D12.5) 

615 FORMAT! IHO, 3X, 'RESIDUAL ERROR' /4X, 1P6D12. 5 ) 

617 FORMAT! IHO, 3X, 'COVARIANCE MATRIX') 

618 FORMAT! IH , 6X, 1P6D12. 5) 

619 FORMAT! IH1,10X, * SIMULATION RESULTS',////) 

IFILSL! 1) )WRITE!6,619) 

620 FORMAT! IH ,3X,'N0RMED POSITION ERROR * • , 1 PlD 1 2 . 5 , 5X , ' 
♦NORMED VELOC 

tITY ERROR * *,IP1D12.5) 

FT=0.0D0 

T1=0.000 

T2=0.0D0 

DO I 1=1,3 

F! I ) = F! I )*CFl 

FT*FT+F( I \**2 

Tl=Tl+XT!I)*XT( i) 

1 T2=T2^XT! I+3)*XT! 1+3) 

FT=DSQRT!FT) 

XS=DSQRT!Tl) 

XV=DSQRT!T2) 

IF!L$L!9))G0 TO 2 
IF!XS.GT.XO)GO TO 3 
XO=XS 

TXS=TIME 

3 IF!XV.GT.XVO)GO TO 2 
XVO=XV 
TXV=TIME 

2 WRITE!6,603)TIME 
WR1TE!6,604)XS,XV,FT 
CHK=DABS!TF-TIME) 

IFICHK.LE. U.IOOO+DELT) ) GO TO 50 
GO TO 60 

50 FTHRST=DSQRT!F! l)**2+F!2)*+2+F!3)**2) 
GUTL=FTHRST/1.0D-04 
GUTTR=DFLOAT! IFIXIGUTL) ) 

QCHK=GUTTR+1.0DO 
TF=TF+QCHK+DELT 
CONTINUE 


60 
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IF(L$L(2) IRETURN 
DO 99901 I$*l.ISLEN 

99901 XE(Ui = XTn$»-XP(I$) 

PNORM=DSQRT ( XE U I ♦★24^XE < 2 ) 3 I ♦♦2 S 

\/NORM=DSQRT« XEU»**Z»Xe(5)**2+XEI6)**2) 

WRITE(6*610)F 

WRITE(6,601IXT 

WR1TE(6»611)XP 

«RITE(6f 612IXE 

WRITE(6,620)PNORM,VNORM 

WRITE(6,613)Z 

WRlTE(6f6l4)ZP 

WRITE(6f615)2E 

WRITE(6,617I 

DO 6 I=l I ISLEN 

6 WRITE(6f 618MPU t J> * J-l*t SLEN) 

WRITE(6,60fll 

99902 L$L{l)=LF 
RETURI'i 

ENTRY TERMIN 

♦ ENTRY 

IF(XS.LT.1.0D-2.AND.XV.LT. l.OD-^)GO TG 4 

IFCTIME.GE.TFIGO TO 5 
RETURN 

4 WRITE(6,605) 

LtL(6)=LF 

RETURN 

5 WRITE(6f609) 

L$U6) = LF 
RETURN 
ENTRY RECAP 

♦ ENTRY 
WRITE(6f606)XO,TXS 
WRITE<6.607) XVOfTXV 
RETURN 

ENTRY CYINIT 
X0= 1.0040 
XV0=l.0D40 

C GRAVITATIONAL ACCELLERATION INVERSE 

CF1=I.ODO/(9.0O665D-O840.64D4*»2I 
RETURN 
END 

SUBROUTINE TARGET(StT) 

IMPLICIT REAL*0 (A-H,0-Zl 
DIMENSION SI3) 

COMMON/VARl/A,RN«f EPS »ETfT STAR tDET 
601 FORMAT(IHO,»CONVERGENCE= »,1PD12.5,» KEPEQM 
EO=ET 
ET=ET+DET 
DO I I=l#100 
SINET=DSIN(ET) 
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C 

c 

c 

c 

c 

c 

c 

c 

c 


COSET=DCOS< ET) 

F=RN«(T+TSTAR)-ET*EPS*SiNET 

DF=EPS*C0SET“1.0D0 

ET=ET-F/DF 

DIF=DABS(F/DF> 

IF ( DI F.LT. I.OD-IOIGO TO 2 

1 CONTINUE 
WRTTEt6t60UOIF 

2 DET=ET-EO 
Sm=A<'(COSET -EPS) 

S(2)=A*OSQRT( l.ODO-EPS^EPSI^'SINET 

S(3)=O.ODO 

RETURN 

SUBROUTINE MINV(AfNfNSQiLfM*8I6A) 
IMPLICIT REALMS (A-HtO-Z) 

« 00000135 

dimension A(NSQ)fLl6)fMl6i 


« 00013200 

description of PARAMETERS 

* A - INPUT^MATRIX, DESTROYED IN COMPUTATION AND R 

REPLACED BY 00013600 

resultant inverse. 

00013700 

N - ORDER OF MATRIX A 
00013800 

BIGA - RESULTANT DETERMINANT 
00013900 

L - WORK VECTOR OF LENGTH N 
00014000 

M - WORK VECTOR OF LENGTH N 
00014100 


♦ 

♦ 

♦ 


NK=-N 
DO 190 K=lfN 

ic 

NK=NK+N 

* 

LCK) = K 
M(K)=K 

* 

KK=NK+K 

* 

BIGA=A(KK ) 
DO 30 J=K,N 


00014200 

00017500 

00017600 

00017700 

00017800 

00017900 

OOOIBOOO 

00018100 


IZ=N*( J“1 ) 


00018200 
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4c 00018300 

00 30 I*K,M 
* 00018400 

IJ=IZ+1 

« 00018500 

10 IF(DABS(BIGAI-0ABS( AH JM ) 20#30»30 
20 BIGA*A(IJI 

4c 00018800 


L(K>=I 

♦ 

M(K)=J 

4 ( 

30 CONTINUE 
4c 


00018900 

00019000 

00019100 


4c 00019200 

INTERCHANGE ROWS 
^ 00019300 

♦ 00019400 

J=L(KI 

4c 00019500 

IF(J-K) 60f60t40 


4c 

00019600 

40 KI=K-N 

4c 

00019700 

DO 50 I=lfN 
4c 

00019800 

KI=KI4^N 

4c 

00019900 

HOLD=-A(KI ) 
4c 

00020000 

JI=KI-K+J 

« 

00020100 

A(Kn=A(JI ) 

4: 

00020200 

50 AIJI) =H0LD 

4c 

00020300 

4c 

00020400 


INTERCHANGE COLUMNS 

4c 00020500 

4t 00020600 

60 1=M(K) 

♦ 00020700 
IFU-KI 90#90f70 

4c 00020BOO 

70 JP=N#(1-1) 

* 00020900 
DO 80 J=1*N 

4c 00021000 

JK=NK4J 

4« 00021100 

JI=JP+J 

4C 00021200 

H0LD=-AIJK) 

4c 00021300 

A(JK)=A(Jl ) 

4c 


00021400 
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80 A(JI) *HOLD 

« 00021500 

C 

* 00021600 

C DIVIDE COLUMN BY MINUS PIVOT 

«T IS 00021700 

C CONTAINED IN BIGA) 

^ 00021800 

C 

m 00021900 

90 IF(BIGA) llOflOOtllO 

«i 00022000 

100 RETURN 

« 00022100 

110 DO 130 I=ltN 

« 00022200 

IF(I-K) 120,130,120 



4I 

00022300 


120 IK=NK-»-I 



* 

00022400 


A( IK)=A(lK»/(-BIGA» 



00022500 


130 CONTINUE 



♦ 

00022600 

c 


00022700 

c 

REDUCE MATRIX 



00022800 

c 

* 

00022900 


DO 160 1=1 

,N 


♦ 

00023000 


IK=NK+I 



* 

00023100 


1J=I-N 



❖ 

00023200 


DO 160 J=1 

,N 


♦ 

00023300 


IJ=IJ+N 




00023400 


IF(I-K) 140,160,140 


* 

00023500 


140 IF(J-K) 150,160,150 


* 

00023600 


150 KJ=IJ-I+K 



4i 

00023700 


A( IJ ) = A( IK 

)«A(KJ)^A( IJ) 



00023800 


160 CONTINUE 




00023900 

c 

♦ 

00024000 

c 

DIVIDE 

ROW BY PIVOT 


♦ 

00024100 

c 


00024200 


KJ=K-N 



♦ 

00024300 


DO 180 J=1,N 


♦ 


(VALUE OF PIVOT ELEMEN 


00024400 



KJ=KJ+N 

* 00024500 
IFU-K) i70tl80#l70 

♦ 00024600 
170 A<KJ)=A<KJ»/BIGA 

4 00024700 

180 CONTINUE 

« 00024800 
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C 

C 

c 


* 

♦ 


00024900 

REPLACE PIVOT BY RECIPROCAL 
00025000 


C 

C 

C 


00025100 
A(KK)«L.0/B1GA 

* 00025200 
190 CONTINUE 

* 00025300 

* 00025400 

FINAL ROW AND COLUMN INTERCHANGE 

* 00025500 

* 00025600 
K-N 

* 00025700 
200 K=IK-l) 

* 00025800 
IF(K) 270i270t210 

* 00025900 
210 I=L(K) 

* 00026000 
IFU-K) 240f240f220 

^ 00026100 

220 JQ=N*(K-1) 

♦ 00026200 

JR=N*( I-l) 

* 00026300 
DO 230 J=1*N 

4 00026400 

JK=JQ+J 

* 00026500 
HOLD=A( JK ) 

4 00026600 

JI=JR+J 

4 00026700 

A(JK)=-A( JI) 

4 00026800 

230 A(JI) =HOLD 

4 00026900 

240 J=M(K) 

4 00027000 

IFU-K) 200,200»250 

4 00027100 

250 KI=K-N 

4 00027200 

DO 260 I=ltN 

4 00027300 

K I=KI+N 

4 


00027400 


H0LD=A(K1I 

00027500 

JI=KI-K^J 

00027600 

A(KIJ=-A( JI 

) 


00027700 

A(JII s=HOLD 

00027800 

GO TO 200 

00027900 

RETURN 

00028000 

END 

t 

00028100 

SUBROUTINE 

CONOBS (NP 

E 

CNBSOOOO 

DIMENSION F(6t6ltGl6t 


- 5 ^- 


«S(6,6) 

DOUBLE PRECISION DETl 


32 


35 


L0= 

NU 


♦ 


CNBS0050 

DO 

32 

1=1, NP 

4t 


CNBS0060 

DO 

32 

J=1,NU 

« 


CNBS0070 

S( I 

f J 

) = G(I,JI 

* 


CNBS0080 

DO 

33 

1=1, NP 

★ 


CNBS0090 

DO 

33 

J=1,NP 

* 


CNBSOlOO 

i Sl( 

It 

J)=F( I,JI 

♦ 


CNBSOllO 

DO 

85 

ITEST=1,2 

♦ 


CNBS0120 

IF 

(LO .EQ. 0) GO TO 90 



CNBS0130 

CALL 

NORML 1NP,L0,S) 

* 


CNBSOIAO 

00 

35 

I = 1,NP 

* 


CNBS0150 

DO 

35 

J = ItLO 

♦ 


CNBS0160 



B(I,J) = S(I,J) 



CNBS0170 

L = 1 



♦ 


CNBS0180 

MOU=NP-l 

* 


CNBS0190 

DO 

AO 

1T=1,M0U 

* 


CNBS0200 


CALL MMUL (NPi NPtLOt S1,S, W) 

♦ CNBS0210 
CALL NORML <NP,LOtW) 

* CNBS0220 
DO 20 I=1,NP 

* CNBS0230 
DO 20 J=lfLO 

# CNBS0240 
Jl=( J>L*LO) 


ItW(6t6)t 



* Cr4eS0250 

Bn,ji)-wcit j) 

* CNBS0260 
20 S(ltJ)=W(ItJ) 

* CNBS0270 
40 L=L+l 

* CNBS02B0 
NDIM=L*L0 

« CNBS0290 

00 41 I = IfNP 

* CNBS0292 
SUM = 0.0 

* CNBS0294 
00 42 J » UNDIM 

4c CMBS0296 

42 SUM = SUM ♦ B(ItJ)442 
4t CNBS029B 

SUM = SQRT(SUM» 

* CNBS0300 

IF (SUM .EQ. 0. ) GO TO 41 
« CNBS0302 

00 45 J = I.NDIM 

* CNBS0304 
45 B(I , J» = B(I ♦ JI/SUM 

4c CNBS0306 


41 CONTINUE 

4c CNBS0308 

DO 1000 It » IfNP 
4C CNBS0310 

DO lOOO JJ - ItNP 

^ CNBS0320 

SUM * 0.0 

4t CNBS0330 

00 lOOl KK * 1,N0IM 
4c CNBS0340 

1001 SUM s SUM * B(II,KK)*B( JJfKK) 


4C CNBS0350 

1000 WdltJJ) = SUM 

41 CN8S0360 

CALL MXINV (NPfWfOETlf lER) 
IF (ITEST ,.GT. U GO TO 87 
4c CNBS0380 

IF ( lER .EQ. 01 GO TO 70 
4« CNBS0390 


GO TO 90 

4C CNBS0420 

70 WRITE(6f300) DETl 

300 FORMAT (26H CONTROLLABLE DET = ,1PE14.4) 

4= CNBS0440 

90 LO=NY 

4< CNBS0450 

DO 93 J=1,NY 
4« CNBS0460 

DO 93 I=1»NP 
4c CNBS0470 

93 S( = J,I I 

4c CNBSO40O 

85 CALL MTRANS (NPiNPfFfSl) 

4: CNBS0490 

87 IF (lER .EO. 0) GO TO 170 
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* 

CNBS0500 



WRI TE(6t 

902) DETl 


lPElA.4i) 

902 FORMAT (26H NOT OBSERVABLE DET = 

t 


CNBS0520 



GO TO 91 





CNBS0530 



170 WRITE(6f 

901) DETl 


1PE14.A) 

901 FORMAT (26H OBSERVABLE DET * 

f 

♦ 

CN8S0550 



91 RETURN 




* 

CNBS0560 



END 




* 

CNBS0570 



SUBROUTINE MATRIX 



* 

MTRXOOOO 



DIMENSION A(6v6)tB(6t6) »C(6f6) 



ENTRY 

MMUL (MPtNPtNUf A,6,C) 



* 

MTRX0520 



DO ll LalfMP 



* 

MTRX0530 



DO ll 1= 

= l,NU 




MTRX05A0 



SUM = 0, 

.0 



♦ 

MTRX0550 



DO 31 J = 

= 1,NP 



* 

MTRX0560 



31 SUM = SUM + A(LiJ)*B(J#n 




MTRX0570 



11 C(L, I) = 

= SUM 




MTRX0580 



RETURN 





MTRX0590 



ENTRY 

MTRANS <M,N,A,B) 



* 

MTRX0790 



o 

o 

o 

= l,M 




MTRX0800 



DO 10 J 

= 1,N 



♦ 

MTRX0810 



10 B(J»I) 

- AUfJ) 



♦ 

MTRX0820 



RETURN 




* 

MTRX0830 



ENTRY 

MXINV (NPf A.DETl, lER) 



DOUBLE 

PRECISION DAI 6f6) fDETUDBl It 

1) 


DO 1 I 

= 1,NP 




MTRX0860 



DO 1 J 

^ 1,NP 



* 

MTRX0870 



1 DA(ttJ) 

= A( It J) 




MTRX0880 



CALL DINVER ( NP , 6, DA , 0, 1 »DB i DETl t I ER ) 


IF (lER 

.NE. 0) RETURN 



* 

MTRX0900 



DO 2 I 

= ItNP 



* 

MTRX0910 



DO 2 J 

= ItNP 




MTRX0920 



2 A(IfJ) 

= DAIltJ) 



* 

MTRX0930 



RETURN 






-62- 


3 


C 

C 

C 


C 


* HTRX0940 
ENTRY NORML (NRtNC.A) 

* MTRXIOOO 


* 


♦ 

IF 


* 

5 


DO 4 J = 1,NC 

MTRXlOlO 
SUM = 0.0 
MTRX1020 
DO 3 I = I, NR 
MTRX1030 

SUM * SUM ♦ A(I,J)»*2 
MTRX1040 
SUM = SQRTISUMI 
MTRXIG50 

(SUM .EQ. 0. ) GO TO 4 
MTRX1060 
DO 5 1 = 1,NR 
MTRX1070 

A(I,J) = A( If JI/SUM 
MTRX1080 


4 CONTINUE 

♦ MTRX1090 


RETURN 

* MTRXllOO 


END 

* MTRXlllO 

SUBROUTINE DINVER ( NA» NADf A » NB t NBDf Bt DE T 1 » I ERROR ) 

THIS SUBROUTINE IS A MODIFICATION OF THE UNIVERSITY OF 
« FLORIDA ONVROOlO 

COMPUTER CENTER'S INVERT • IT USES DOUBLE PRECISION AN 
*0 HAS BEEN DNVR0020 
RENAMED DINVER. C FOSHA 2-69 
’S' DNVR0030 

DIMENSION A(NADfNAD)fB(NBDfNBD) fBD(6)f IN0EX(6) 

DOUBLE PRECISION A, B , BD, SAVE t P I VOT , DETl 

DET1=1.000 

lERROR = 0 

* DNVR0070 

00 130 I = I, NA 

^ DNVR0080 

PIVOT = O.ODO 

* DNVR0090 

SEARCH FOR PIVOTAL ELEMENT 
DNVROlOO 

DO 60 J = I, NA 

* DNVROllO 

IF (DABS(A( Jf I ) ) .LE. DABS(PIVOT)) GO TO 60 

* DNVR0120 
PIVOT = AUfI) 

* DNVR0130 
INDEX! I ) = J 

0NVR0140 

60 CONTINUE 

* ONVR0150 

IF (DABS(PIVOT» .LT. 1. D-6» GO TO 250 
DNVR0160 

IF ( INDEX! I ) .EO. I ) 30 TO 90 

* DNVR0170 
DET1=-DET1 


C 


DIAGONAL 


INTERCHANGE ROWS TO PUT PIVOTAL ELEMENT ON D 
DNVR0190 
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DO 80 L * !♦ WA 

« ONVRO2O0 

SAVE = AUtU 
4t 01^47802 10 

A(1«L^ » AdNOEXd)* U 
<c DNVR0220 

80 AdNDEXd)f U = SAVE 

« DIWR02 30 

90 DET1=DET1*PIV0T 
Adti) = l.ODO 

* DNVR02SO 

DO 91 KK*l,NA 

« DIVVR0260 

91 Ad,KKJ=Ad,KK)/PtVOT 

♦ 0NVR0270 

C REDUCE NON-PIVOTAL ROWS 

* DNVR0280 
DO 130 LJ » t» NA 

DNVR0290 

IF (LJ .EQ* li GO TO 130 

« DNVR0300 

SAVE - A(LJ» I) 

« 0NVR03I0 

A(LJtl) = O.ODO 
4c 0NVR0320 

DO 120 K * li NA 
4C 0NVR0330 

120 A(LJ»K) * A(LJ,KI - SAVE * Ad.K) 

♦ 0NVR0340 


CONTINUE 

4= 

DNVR0350 

INTERCHANGE COLUMNS 


DNVR0360 

NAl=NA-»-l 

4c 

0NVR0370 

DO 160 

KKK = 1, NA 


♦ DNVR0380 

K = NAl - KKK 


« DIVVR0390 

IF dNDEX(K> .EQ. Kl GO TO 160 

* D^5VR0400 

00 98 L 1# MA 

* DNVR0410 


SAVE = A(LfK) 

* DNVR0420 
A(L,K) = A(L, IIVDEX(K)) 

« DMVR0430 

98 A(L, INDEXIKd = SAVE 

♦ DNVROAAO 


160 

C 

c 


CONTINUE 


4c 


0NVR0A50 

A INVERSE IS NOW STORED IN A 

4 : 


0NVR0460 



FIND SOLUTION VECTORS FOR ALL CONSTANT VECTO 

♦ RS 

INPUT 

DNVR0470 

IF 

(NB .LE 

. 0) RETURN 

4t 


DNVR0490 

DO 

190 

K = It NB 

4c 


DNVR0^90 

DO 

180 

I = if na 



* DNVR0500 
BD( I ) = O.ODO 

* DNVROSlO 
DO 180 J * if NA 

* ONVR0520 

180 BD(I» = BD(I» f AnfJt«BrjfK) 

* DNVR0530 
DO 190 I = 1# NA 

« DNVR0340 

190 B( I tK ) = B0( I) 

* DNVR0550 

C SOLUTION VECTORS NOW IN B 

* DNVR0560 
RETURN 

* DNVR0570 

C IF CONTROL REACHES 250f MATRIX IS SINGULAR 

* DNVR0580 
250 lERROR = +1 

* 0NVR0590 
DETI=0.0D0 

RETURN 

* DNVR0610 
END 

* DNVR0620 
SUBROUTINE INTGR 
IMPLICIT REALMS (A-H.O-Z) 

LOGICAL*! L$LfL$E,LMON*LF,LT 
LOGICAL*! LS1,LS2 

COMMON/ VtRBLE/CRAPi 6! , XT ( 6 ) ♦ XE ( 6 ) t Z ( 6 ) , ZPI 6 ) , ZE ( 6 ) 
COMMON/T$MER/DELT,OT*TIME,TI ,TFfN, ISLENflOLEN 
COMMON/ SY$TEM/L$L(<^0)tL$EC lOJ 
C0MM0N/M$NITR/LM0N(20) 

COMMON/NOI $E/IRAN( 10) iDG( 10) tRFRAF<6) 
C0MM0N/0FFSET/C(3) 

CQMMON/XPROP/XPNEWI 6) 

DIMENSION XINTI6) ,SUM(6) 

DATA LF,LT/F,T/ 

L$L( I2)=LT 
700 CONTINUE 

TIME=TIME-DELT 
DO 1 ICYCLE=1,N 
DO 33 1=1, ISLEN 
33 SUM(I)=O.ODO 
L$LI 10)=LT 
DO 10 11=1,4 
LS1=II.EQ.2. OR.Il.EQ.3 
LS2=Il.EQ.4 
L$Lai) = Il.EQ.3 
F=F1 
FS=F5 

IF(LS1)F=F2 
IF(LSI)FS=F3 
IF(LS2)FS=F4 
TS=TIME+DT*FS 
DO 20 1=1, ISLEN 
20 XINTII )=XT( I )+FS*XINT( I ) 

DO 31 1=1, NPl 
J=I-1 

IF(J.GT.O) GO TO 2 
DUM=DOFX$N( J,TS) 


-65- 


DUMsGOFX$N( JftSI 
L$U 12)=LF 
GO TO 3l * , 

2 XINT( J)*DT*(DOFXN< JKGOFXNC » 
SUM( J) = SUM( J)^F*XIMTU» 

31 CONTINUE 

L$L( 10)=LF 

10 CONTINUE 
TIME=T1ME>DT 

DO II I*l»JSLEf*4 

11 xTt n=xTtn4^suMni 
I CONTINUE 

701 CONTINUE 
RETURN 
ENTRY INTINI 
Fl=1.0DO/6.000 
F2=2.0D0*F1 
F3=I.ODO/2.000 
F4=1.0D0 


F5=O.ODO 
NPl = ISLEN4-l 
DO 32 I=l*ISLEN 
32 XINTm = XT(I) 

RETURN 

END 

FUNCTION DOFX$N(JfTS) 

IMPLICIT REAL*8( A-Hf O-Z) 

COMMON/viRBLE/CRAPlM. 

COMMON/ TiMER/DELTfDT t T 1 ME 1 1 1 , TF t N f I SLENf I OLEN 
COMMON/ SY$TEM/L$L( 40 ) tLtE( 10) 

COMMON/M$NITR/LMON(20) ncoAciAi 

COMMON/NOI $E/ IRANI 10) f DGI 10) 

COMMON/VARl/Af RNf EPS f EOfT STAR f TRASH 

COMMON/NEWDET/DETN 
COMMON/OFF SET/C I 3) 
dimension 0(3)*S(3) 


data LF,LT/F#T/ w 

99601 FORMAT! 1H0» I4t • IMPROPER If^^EX 
IF I .NOT.LSLI 11 ) )CALL TARGENIS^ 


♦DOFX* ) 


S2=0.0D0 


D2=0.000 
DO I I=lf3 
D( I )=S( I )+C( I )+XT( I ) 
D2=02+DI I )**2 
1 S2=S2+S(I)*»2 

DN=DSQRTI02) 
SN=DSQRT(S2) 
RATI=GM/(SN*SN*SN) 
RAT2=(SN/DN)<‘*3 

dofx$n=o.odo 


RETURN 

GO^TO (99999^^999,99999,99998,99998.99998), J 

WRITEI6, 99601) J 

dofxn=o.odo 

L$E(2)=LT 

RETURN 

99999 D0FXN=XT (J-*-3 ) 

RETURN 
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99998 DOFXN=RATI*( S( J-3)-D( J-3) ♦RATZ I 
RETURN 

ENTRY DXININIJ) 

GM=9.90549D05 

DETN=1.0D-3 

DXININ=O.ODO 

RETURN 

END 

FUNCTION GOFXSNiJtTSI 
IMPLICIT REAL*8(A-H#0-n 
LOGICAL*! L$L#L$E*LM0N,LF,LT 

C0MM0N/VSRBLE/XT(6»#XP(6) ♦XEI6I tZl6) ,ZP(6» t ZEI6) 
COMMON/T$MER/DELTfOT,TlMEt Tl#TF,Nt ISLENo lOLEN 
COMMON/SYtTEM/LSLUOl .LiEIlO) 
COMMON/M$NITR/LMONI20I 
COMMQN/NOI$E/IRANUOI»DG( 10)|RFRAF<6) 
COMMON/FORCE/F( 3» 

C0MM0N/0FFSET/C(3» 

COMMON/XPROP/XPNEWI6I 
DATA LF,LT/F,T/ 

9960! FORMATUHOtlA,* IMPROPER INDEX ♦GOFXM 
IF(L$L( 12) )TIMl=TS 
TAU0=TF-TIM1 
TAU=TF-TS 

TRAT1=(6.0D0/TAU0**2)*( I.0O0-2.0D0*! TAU/TAUO) ) 
TRAT2=I2.0D0/TAUO)*( I . ODO-3. 000* ( TAU/TAUO) ) 
G0FXtN=0*0D0 
RETURN 

ENTRY GOFXNIJ) 

GO TO ( 99999, 99999a99999, 99998, 99998f99998) fJ 

WRITEI6, 99601) J 

L$E( 3)=LT 

GOFXN=O.ODO 

RETURN 

99999 GOFXN=O.ODO 
RETURN 

99998 F( J-3) = TRAT1*XPNEW( J-3)-»-TRAT2*XPNEW(J) 

GOFXN=F( J-3) 

RETURN 

ENTRY GXININ(J) 

GXININ=O.ODO 

RETURN 

END 

SUBROUTINE TARGEN(S.T) 

IMPLICIT REAL*8 (A-H,0-Z) 

DIMENSION Sii) 

COMMON/ VAR I/A,RN,EPS,ET,TSTAR, TRASH 
COMMON/NEWDET/OETN 

601 FORMAT! IHO,*CONVERGENCE=»,lPOl2. 5, • KEPEQ* ) 
EO=ET 

ET=ET+DETN 
00 1 1 = 1,100 
SINET=DSIN(ET) 

COSET=DCOS(ET) 

F=RN*(T+TSTAR)-ET+EPS*SINET 

DF=EPS*COSET-1.0D0 

ET=ET-F/DF 

DIF=DABS(F/DF) 

IFIDIF.lt. 1.0D-10)GO TO 2 
1 CONTINUE 
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WRITE(6t60UDIF 
2 DETN=ET-EO 

sa )=A*(COSET-EPS> 

S(2)=A#DSQRT( 1.0D0-EPS*EPSI*SINET 

S(3)=O.ODO 

RETURN 

END 

//GD.SYSIN DO * 

TEST RUN 

TFFTTTTTFTTTTTTTTTTTTTTTTTTTTTTTTTTTTTTT 

FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF 


10 6 A 

3A.0 39,0 

0.01 

O.l 

.366447 

.275514 

.192315 

-.141411 -.106223 -.072940 

3318.76000.0052056 

.846765 

6.0 U42.8 

0.01 

0.01 

0.01 




